from PyQt5 import QtCore, QtGui, QtWidgets
from PyQt5.QtGui import QIcon, QPalette, QBrush, QPixmap, QFont
from PyQt5.QtWidgets import QApplication, QDialog, QHBoxLayout, QVBoxLayout
import sys
import matplotlib
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg
from matplotlib.backends.backend_template import FigureCanvas
from matplotlib.figure import Figure
import numpy as np
from PIL import Image
import os
import matplotlib.pyplot as plt
from pylab import mpl

mpl.rcParams['axes.unicode_minus'] = False  # 正常显示负号
class Canvas(FigureCanvasQTAgg):
    def __init__(self, parent=None, width=5, height=4, dpi=100):
        self.fig = Figure(figsize=(width, height), dpi=dpi)
        self.fig = Figure(figsize=(width, height), dpi=dpi)
        FigureCanvasQTAgg.__init__(self, self.fig)
        self.axes = self.fig.add_subplot(111)
        self.setParent(parent)
        self.wall_x = None
        self.wall_y = None
        self.updata_figure()
        self.color = ['r','c', 'b','m','y','lightcoral','darkgreen','blueviolet','r','c', 'b','m','y']
        FigureCanvasQTAgg.updateGeometry(self)
        # self.fig.canvas.mpl_connect('button_press_event', self.on_press)

    def updata_figure(self, x =None, y = None, name=None,wall_list_x = None,wall_list_y = None):
        self.axes.cla()
        self.axes.set_title(name, fontsize='small')
        self.axes.set_xlim([-10, 10])
        self.axes.set_ylim([45, 60])
        self.axes.tick_params(labelsize=5)  # 设置xy字体大小
        if x!= None:
            self.axes.scatter(x, y, marker='.', c=self.color[0:len(x)], cmap='jet', s=1, linewidths= None)
        if ( wall_list_x != None and wall_list_y != None):
            self.axes.scatter(wall_list_x, wall_list_y,  marker='|',c='k', s=1.5,linewidths=2)
        self.draw()
    def updata_figure_fusion(self, x =None, y = None, name=None,wall_list_x = None,wall_list_y = None):
        self.axes.cla()
        self.axes.set_title(name, fontsize='small')
        self.axes.set_xlim([-10, 10])
        self.axes.set_ylim([45, 60])
        self.axes.tick_params(labelsize=5)  # 设置xy字体大小
        print(name,wall_list_x)
        if x!= None:
            self.axes.scatter(x, y, marker='.', c=self.color[0:len(x)], cmap='jet', s=1, linewidths= None)
        if ( wall_list_x != None and wall_list_y != None):
            self.axes.scatter(wall_list_x, wall_list_y,  marker='|',c='k', s=1.5,linewidths=3)
        self.draw()

class locationUi(QDialog):
    signal_write_fusion_msg = QtCore.pyqtSignal(str)  # 数据融合写信息
    def __init__(self, radar_person_location,laser_person_location,fusion_person_location,wall_list_x = None ,wall_list_y = None):
        super(locationUi, self).__init__()

        self._translate = QtCore.QCoreApplication.translate
        self.setObjectName("TCP-UDP")
        self.setAcceptDrops(False)
        self.setSizeGripEnabled(False)
        font = QtGui.QFont()
        font.setFamily("Yuppy TC")
        font.setPointSize(8)
        self.setFont(font)
        self.wall_list_x = wall_list_x
        self.wall_list_y = wall_list_y

        self.pushButton_error_location = QtWidgets.QPushButton()  #

        self.label_location_radar = QtWidgets.QLabel()  #
        self.label_location_laser = QtWidgets.QLabel()  #
        self.label_location_fusion = QtWidgets.QLabel()  #

        self.lineEdit_location_radar = QtWidgets.QLineEdit()  #
        self.lineEdit_location_laser = QtWidgets.QLineEdit()  #
        self.lineEdit_location_fusion = QtWidgets.QLineEdit()  #
        self.lineEdit_location_radar.setFixedWidth(150)
        self.lineEdit_location_laser.setFixedWidth(150)
        self.lineEdit_location_fusion.setFixedWidth(150)

        self.textBrowser_recv_fusion = QtWidgets.QTextBrowser()  # 文本浏览框
        self.textBrowser_recv_point = QtWidgets.QTextBrowser()  # 文本浏览框
        self.textBrowser_recv_error = QtWidgets.QTextBrowser()  # 文本浏览框
        # self.textBrowser_recv_fusion.setFixedWidth(150)
        self.myfigure_rader = Canvas(self, dpi=150)  # 雷达画布
        self.myfigure_rader.axes.set_title('Radar', fontsize='small')

        self.myfigure_laser = Canvas(self, dpi=150)  # 激光画布
        self.myfigure_laser.axes.set_title("Laser", fontsize='small')
        self.myfigure_laser.setFixedHeight(250)

        self.myfigure_fusion = Canvas(self, dpi=150)  # 总画布
        self.myfigure_fusion.updata_figure(name="Fusion")
        # self.myfigure_fusion.setFixedWidth(550)
        self.myfigure_fusion.setFixedHeight(350)

        self.h_box_up = QHBoxLayout()  #
        self.h_box_1 = QHBoxLayout()  #
        self.h_box_2 = QHBoxLayout()  #
        self.h_box_3 = QHBoxLayout()  #
        self.h_box_4 = QHBoxLayout()  #
        self.h_box_5 = QHBoxLayout()  #
        self.v_box_left = QVBoxLayout()  # 左侧布局
        self.v_box_right = QVBoxLayout()  # 左侧布局
        self.v_box_all = QVBoxLayout()  # 左侧布局

        self.layout_ui()
        self.ui_translate()
        if radar_person_location != []:
            self.show_location_2('雷达坐标',radar_person_location,self.myfigure_rader,'Radar',1)
        if laser_person_location != []:
            self.show_location('激光坐标',laser_person_location,self.myfigure_laser,'Laser',1)
        if fusion_person_location != []:
            self.show_location_2('融合坐标',fusion_person_location,self.myfigure_fusion,'Fusion',2)
        self.connect()
        self.show_point()

    def show_location(self,location_name,person_location,myfigure,myfigure_name,flag):
        x_array = []
        y_array = []
        msg = location_name + '\n'
        # print(person_location)
        for i in range(0, len(person_location)):
            if len(person_location[i]) > 6:
                point_x = person_location[i][0][0]
                point_y = person_location[i][0][1]
                x_array.append(float(point_x))
                y_array.append(float(point_y))
                msg = msg +  format(point_x, '.3f') + ',' + format(point_y, '.3f')  + ',' + '\n'
        if flag == 1:
            myfigure.updata_figure(x_array,y_array,myfigure_name,self.wall_list_x,self.wall_list_y)
        if flag == 2:
            myfigure.updata_figure_fusion(x_array, y_array, myfigure_name, self.wall_list_x, self.wall_list_y)
        self.write_fusion_msg(msg)

    def show_location_2(self,location_name,fusion_list,myfigure,myfigure_name,flag):
        x_array = []
        y_array = []
        msg = location_name + '\n'
        # print(person_location)
        for i in range(len(fusion_list)):
            object_radar_x = format(fusion_list[i][0], '.3f')
            object_radar_y = format(fusion_list[i][1], '.3f')
            x_array.append(float(fusion_list[i][0]))
            y_array.append(float(fusion_list[i][1]))
            msg = msg + object_radar_x + ',' + object_radar_y + ',' + '\n'
        msg = msg + '\n'
        if flag == 1:
            myfigure.updata_figure(x_array,y_array,myfigure_name,self.wall_list_x,self.wall_list_y)
        if flag == 2:
            myfigure.updata_figure_fusion(x_array, y_array, myfigure_name, self.wall_list_x, self.wall_list_y)
        self.write_fusion_msg(msg)

    def layout_ui(self):
        self.h_box_1.addWidget(self.label_location_radar)
        self.h_box_1.addWidget(self.lineEdit_location_radar)
        self.h_box_1.addStretch(1)  # 增加伸缩量

        self.h_box_2.addWidget(self.label_location_laser)
        self.h_box_2.addWidget(self.lineEdit_location_laser)
        self.h_box_2.addStretch(1)  # 增加伸缩量

        self.h_box_3.addWidget(self.label_location_fusion)
        self.h_box_3.addWidget(self.lineEdit_location_fusion)
        self.h_box_3.addWidget(self.pushButton_error_location)

        self.h_box_4.addWidget(self.myfigure_rader)
        self.h_box_4.addWidget(self.myfigure_laser)

        self.h_box_5.addWidget(self.textBrowser_recv_fusion)
        self.h_box_5.addWidget(self.textBrowser_recv_point)

        self.v_box_left.addLayout(self.h_box_1)
        self.v_box_left.addLayout(self.h_box_2)
        self.v_box_left.addLayout(self.h_box_3)

        self.v_box_left.addLayout(self.h_box_5)
        self.v_box_left.addWidget(self.textBrowser_recv_error)

        self.v_box_right.addLayout(self.h_box_4)
        self.v_box_right.addWidget(self.myfigure_fusion)

        self.h_box_up.addLayout(self.v_box_left)
        self.h_box_up.addLayout(self.v_box_right)

        self.v_box_all.addLayout(self.h_box_up)

        self.setLayout(self.v_box_all)

    def ui_translate(self):
        self.setWindowTitle(self._translate("TCP-UDP", "位置信息"))
        self.pushButton_error_location.setText(self._translate("TCP-UDP", "计算误差"))
        self.label_location_radar.setText(self._translate("TCP-UDP", "雷达坐标"))
        self.label_location_laser.setText(self._translate("TCP-UDP", "激光坐标"))
        self.label_location_fusion.setText(self._translate("TCP-UDP", "融合坐标"))

    def connect(self):
        self.pushButton_error_location.clicked.connect(self.click_error)  # 显示表格 按钮

    def click_error(self):
        person_location_radar = str(self.lineEdit_location_radar.text()).split(';')
        person_location_laser = str(self.lineEdit_location_laser.text()).split(';')
        person_location_fusion = str(self.lineEdit_location_fusion.text()).split(';')
        error_1 = ''
        error_2 = ''
        error_3 = ''
        if person_location_radar != ['']:
            for i in range(0,len(person_location_radar)):
                person_location = person_location_radar[i].split(',')
                error_1 = error_1 + name_radar_error(float(person_location[0]), float(person_location[1]), radar_true_value,
                                           left_corner_x, left_corner_y, int(person_location[2]), '雷达')  # 雷达在雷达
        if person_location_laser != ['']:
            for i in range(0,len(person_location_laser)):
                person_location = person_location_laser[i].split(',')
                if int(person_location[2]) <= len(laser_true_value):
                    error_2 = error_2 + name_laser_error(float(person_location[0]), float(person_location[1]), laser_true_value,
                                               left_corner_x, left_corner_y, int(person_location[2]), '激光')  # 激光在激光

        if person_location_fusion != ['']:
            for i in range(0,len(person_location_fusion)):
                person_location = person_location_fusion[i].split(',')
                error_3 = error_3 + name_radar_error(float(person_location[0]), float(person_location[1]), radar_true_value,
                                           left_corner_x, left_corner_y, int(person_location[2]), '融合')  # 融合在雷达
                # if int(person_location[2]) <= len(laser_true_value):
                #     error_3 = error_3 + name_laser_error(float(person_location[0]), float(person_location[1]), laser_true_value,
                #                                          left_corner_x, left_corner_y, int(person_location[2]),
                #                                          '融合')  # 融合在激光
        self.write_error_msg('--------------------------------------------' + '\n')
        self.write_error_msg(error_1 )

        self.write_error_msg(error_2 )

        self.write_error_msg(error_3 )
    def show_point(self):
        msg = ''
        for i in range(len(radar_true_value)):
            msg = msg + str(i+1) + ':'
            x = left_corner_x + radar_true_value[i][0] + 0.1
            y = left_corner_y + radar_true_value[i][1] + 0.1
            msg = msg + format(x,'.3f') + ',' +  format(y,'.3f') + '\n'
        self.write_point_msg(msg)
    def write_fusion_msg(self, msg):
        self.textBrowser_recv_fusion.insertPlainText(msg)
        # 滚动条移动到结尾
        self.textBrowser_recv_fusion.moveCursor(QtGui.QTextCursor.End)

    def write_point_msg(self, msg):
        self.textBrowser_recv_point.insertPlainText(msg)
        # 滚动条移动到结尾
        self.textBrowser_recv_point.moveCursor(QtGui.QTextCursor.End)

    def write_error_msg(self, msg):
        self.textBrowser_recv_error.insertPlainText(msg)
        # 滚动条移动到结尾
        self.textBrowser_recv_error.moveCursor(QtGui.QTextCursor.End)

radar_true_value = np.array(
    [[0.48, 1.57], [2.47, 1.38], [5.4, 2.74], [5.4, 2.13], [5.4, 1.47], [7.16, 2.72], [7.56, 2.0], [10.17, 0.94],
     [10.02, 2.47]])
laser_true_value = np.array([[243, -277], [258, -77], [158, 216], [219, 216], [285, 216]])
left_corner_x = -5.8
left_corner_y = 50.8


def name_radar_error(in_x, in_y, radar_true_value, left_corner_x, left_corner_y, num,name):
    error_x = in_x - left_corner_x - radar_true_value[num - 1][0] - 0.1
    error_y = in_y - left_corner_y - radar_true_value[num - 1][1] - 0.1
    error = (error_x ** 2 + error_y ** 2) ** 0.5
    error_data = name + '在雷达' + str(num) + '号点的位置为(' + str(in_x) + ',' + str(in_y) + ')，x方向的误差为' \
                 + format(error_x, '.3f') + 'm，y方向的误差为' + format(error_y, '.3f') \
                 + 'm，总误差为' + format(error, '.3f') + 'm'+ '\n'
    radar_file = None
    if name == '雷达':
        radar_file = open('./data/radar_error.txt', mode='a')
    elif name == '融合':
        radar_file = open('./data/fusion_error.txt', mode='a')
    radar_file.write(error_data)
    radar_file.close()
    return error_data


def name_laser_error(in_x, in_y, laser_true_value, left_corner_x, left_corner_y, num,name):

    x, y = laser_to_radar(laser_true_value[num - 1][0], laser_true_value[num - 1][1])
    error_x = in_x - left_corner_x - x
    error_y = in_y - left_corner_y - y
    error = (error_x ** 2 + error_y ** 2) ** 0.5

    error_data =  name + '在激光' + str(num) + '号点的位置为(' + str(in_x) + ',' + str(in_y) + ')，x方向的误差为' \
                 + format(error_x, '.3f') + 'm，y方向的误差为' + format(error_y, '.3f') \
                 + 'm，总误差为' + format(error, '.3f') + 'm' + '\n'
    laser_file = None

    if  name == '激光':
        laser_file = open('./data/laser_error.txt', mode='a')
    elif name == '融合':
        laser_file = open('./data/fusion_error.txt', mode='a')
    laser_file.write(error_data )
    laser_file.close()
    return error_data



def laser_to_radar(laser_x, laser_y):
    x = 3.5 + laser_y * 0.01
    y = 4.26 - laser_x * 0.01
    return x, y

if __name__ == '__main__':
    app = QApplication(sys.argv)
    msg = [(10,50)]
    object_list =  [(0.430, 10.013)]
    ui = locationUi(object_list,msg,object_list)
    ui.show()
    sys.exit(app.exec_())
